import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Map;

import com.fasterxml.jackson.core.JsonParseException;
import com.fasterxml.jackson.databind.JsonMappingException;

/**
 * @author Remi Destigny
 * The path class is a helper function used by the robot to access the different points of the path. 
 * It includes :
 * - Parsing a JSON path file
 * - Calculating the next goal point for both pure pursuit and follow the carrot algorithms
 * - Removing old points that the robot has already crossed
 * 
 */
public class Path {
	/**
	 * Max point number used to find the goal point
	 */
	public static final int MAX_POINT = 70;
	/**
	 * Size of the buffer used to read the JSON path file
	 */
	public static final int BUFFER_SIZE = 1024;

	private ArrayList<Vector> _pathArray;
	private int _currentGoalPointPosition;

	/**
	 * Constructor for the Path class, extract GPSCoordinates from a JSON file  and put them in an ArrayList
	 * @param pathFile The JSON file from which the coordinates will be extracted 
	 * @throws IOException 
	 */
	public Path(String pathFile) throws JsonParseException, JsonMappingException, IOException {
		// LOGS
		System.out.println("Loading JSON path file : " + pathFile);
		_pathArray = new ArrayList<Vector>();
		_currentGoalPointPosition = -1;
		StringBuilder jsonContent = new StringBuilder();
		// Opening the file
		File f = new File(pathFile);

		// Buffered file reading
		FileReader fr = new FileReader(f);
		char[] buffer = new char[BUFFER_SIZE];
		int read = 0;
		do {
			jsonContent.append(buffer, 0, read);
			read = fr.read(buffer);
		} while (read >= 0);
		// Building the JSON string and parsing it
		String jsonString = jsonContent.toString();
		ArrayList<Object> path = JSONhelper.decodeJsonArray(jsonString);

		// LOGS
		final int pointNumber = path.size();
		int pointCounter = 0;
		// Converting the JSON input to GPSCoordinates
		for (Object o : path) {
			Map<String, Object> pose = (Map<String, Object>) ((Map<String, Object>) o).get(JSONhelper.LOCALIZATION_POSE);
			Map<String, Object> position = (Map<String, Object>) pose.get(JSONhelper.LOCALIZATION_POSITION);
			Vector coordinates = new Vector((Double) position.get("X"), (Double) position.get("Y"));
			_pathArray.add(coordinates);
			// LOGS
			pointCounter++;
			if (pointCounter % 100 == 0) {
				System.out.println("Computing path points " + pointCounter + " / " + pointNumber);
			}
		}
		// LOGS
		System.out.println("JSON path file fully parsed, " + pointNumber + " points computed");
		dumpPath(_pathArray, "oldPath.dat");
	}

	/**
	 * Try to find the best farthest point to set it as the goal point 
	 * @param lookahead The max distance at which the point can be
	 * @param r The robot 
	 */
	public void findFarthestPoint(double lookahead, Robot r) {
		double farthestDistance = 0;
		int i = 0;
		Vector transformedPoint = null;
		// Test for every point in the path 
		for (Vector v : _pathArray) {
			// Transform the point to be in the robot system of coordinates
			transformedPoint = MathUtils.pathPointToRobotCoordinates(r, v);
			// Compute the distance from the robot to the point
			double distance = transformedPoint.getNorm();
			// If the point is interesting ( further than the latest better point but still in the [EPSILON_DISTANCE, LOOKAHEAD] range ) 
			// we save it 
			if (distance > farthestDistance && distance < lookahead && distance > Robot.EPSILON_DISTANCE) {
				farthestDistance = distance;
				_currentGoalPointPosition = i;
			}
			i++;
			// If the points start being further than the lookhead, there is no reason to keep testing since
			// they will all be discarded
			if (distance > lookahead || ( i >= MAX_POINT && farthestDistance > Robot.EPSILON_DISTANCE) ) {
				break;
			}
		}	
		// No point found, we choose the last one
		if (_currentGoalPointPosition == -1) {
			// Make sure we are not further than the end of the list
			if ( i < _pathArray.size())
				_currentGoalPointPosition = i;
			// If the list is over, get the last point in the list
			else
				_currentGoalPointPosition = _pathArray.size() - 1;
		}		
	}

	/**
	 * Remove every point before the current goal point and the goal point from
	 * the point array 
	 */
	public void removePointUntilGoalPoint() {
		int position = _currentGoalPointPosition;
		for (int i = position; i >= 0; i--) {
			_pathArray.remove(0);
		}
	}

	/**
	 * Return the current goal point of the path in the robot system of coordinates
	 * @param r The robot
	 * @return A Vector with x and y coordinates in the robot system of coordinates 
	 */
	public Vector getGoalPoint(Robot r) {
		if (_currentGoalPointPosition >= 0 && _currentGoalPointPosition < _pathArray.size())
			return MathUtils.pathPointToRobotCoordinates(r, _pathArray.get(_currentGoalPointPosition));
		else
			return MathUtils.pathPointToRobotCoordinates(r, _pathArray.get(_pathArray.size() - 1));
	}

	/*
	 * Debug function used to print points in a gnuplot format 
	 * File should be readable on gnuplot : 
	 * load 'name.dat'
	 * plot with labels 
	 */
	public void dumpPath(ArrayList<Vector> pathArray, String name) {
		try {
			FileWriter fw = new FileWriter(name);
			int i = 0;
			for (Vector v : pathArray) {
				fw.append(v.x + " " + v.y + " " + i + "\n");
				i++;
			}
			fw.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

	}

	/**
	 * Utility function checking the number of point left in order to tell the robot the path is finished
	 * @return True if there is less than 5 point left in the path array, False otherwise
	 */
	public boolean noMorePoint() {
		return _pathArray.size() <= 5;
	}
}

